#include <Max3421e.h>
#include <Usb.h>
#include <AndroidAccessory.h>


const int analogpin0 = A0;  // select the pin for analog0 input
const int analogpin1 = A1;  // select the pin for analog0 input
const int motor1p = 2;
const int motor1n = 3;
const int motor2p = 4;
const int motor2n = 5;



AndroidAccessory acc("isis re-direct",
"h2h",
"h2h Arduino Board",
"1.0",
"http://www.android.com",
"0000002212345678");

void setup();
void loop();



class FingerPulseMotor {
  static const  int  strongth = 128;
  int pin1;
  int pin2;
  unsigned long pretime;
  int  state; //{0,1,2}
  unsigned long  lastPStartTime;
public:
  FingerPulseMotor(int p1, int p2 ) : 
  pin1(p1), pin2(p2), pretime(0), state(0), lastPStartTime(0)  {

  }
  void  initpin() {
    pinMode(pin1, OUTPUT);
    analogWrite(pin1, 0);
    pinMode(pin2, OUTPUT);
    analogWrite(pin2, 0);
    state =0;
    lastPStartTime =0;
  }
  void inittime( unsigned long now ) {
    pretime = now;
    state =0;
    lastPStartTime =0;
    analogWrite(pin1, 0);
    analogWrite(pin2, 0);
  }
  void update( unsigned long now ) {
    if (state == 2 && 300 < (now - lastPStartTime)) {
      analogWrite(pin1, 0);
      analogWrite(pin2, 0);
      state = 0;
    }
    else if (state == 1 && 150 < (now - lastPStartTime)) {
      analogWrite(pin1, 0);
      analogWrite(pin2, strongth);
      state = 2;
    }
  }

  void  turnon(unsigned long now) {
    analogWrite(pin1, strongth);
    analogWrite(pin2, 0);
    lastPStartTime = now;
    state = 1;

  }
  void  turnoff() {
      analogWrite(pin1, 0);
      analogWrite(pin2, 0);
      state = 0;
  }
};



class Pulse {
  static const int ptsize = 128;
  static const int ep = 20;

  int  pin;
  int ucount;
  int prevval;
  int vmax;
  int vmin;
  unsigned long  pulsetime[ptsize];
  int pulsetimepos;
  int prevoneminitespos;
  unsigned long  prevpulsetime;
  int  pcd_avg;

  int pc;

  int interval;
  int pcd;

public:
  Pulse( int p) {
    pin = p;
    ucount = 0;
    prevval = 0;
    vmax = 0;
    vmin = 1023;

    pulsetimepos = 0;
    prevoneminitespos = 0;
    pcd_avg = 60;
    memset(pulsetime, ptsize, 0);

  }
  void  initpin() {
    pinMode(pin, INPUT);
    digitalWrite(pin, HIGH);
  }
  void inittime( unsigned long now) {
    prevpulsetime = now;
    ucount = 0;
    prevval = 0;
    vmax = 0;
    vmin = 1023;

    pulsetimepos = 0;
    prevoneminitespos = 0;
    pcd_avg = 60;
    memset(pulsetime, ptsize, 0);
  }

  boolean checkpulse( unsigned long now ) {
    int a_input = analogRead(pin);

    boolean p = false;
    if (a_input < vmin) {
      vmin = a_input;
    }
    if (vmax < a_input) {
      vmax = a_input;
    }
    if ((prevval < (a_input + ep))) {
      ucount++;
    }
    else if (a_input < prevval) { // peak 
      if (7 < ucount) {
        if (80 < (vmax - vmin)) {
          p = true;
        }
        ucount = 0;
        vmax = 0;
        vmin = 1023;
      }
      else{
        ucount = 0;
        vmax = 0;
        vmin = 1023;
      }
    }
    else{
      ucount = 0;
      vmax = 0;
      vmin = 1023;
    }
    prevval = a_input;    
    if (p) {

      pulsetime[pulsetimepos] = now;
      if (60000 < (pulsetime[pulsetimepos] - pulsetime[prevoneminitespos])) {
        while (60000 < (pulsetime[pulsetimepos] - pulsetime[prevoneminitespos])) {
          prevoneminitespos++;
          if (ptsize <= prevoneminitespos) {
            prevoneminitespos = 0;
          }
        }
      }
      if (prevoneminitespos != pulsetimepos) {
        pc = (prevoneminitespos < pulsetimepos) ? (pulsetimepos - prevoneminitespos) : (ptsize + pulsetimepos - prevoneminitespos);
      }

      Serial.print("\r\n-- ");
      Serial.print(pin);
      Serial.print(":");
      Serial.print(pulsetimepos);
      Serial.print(" ");
      Serial.print(pulsetime[pulsetimepos]);
      Serial.print("/");
      Serial.print(prevoneminitespos);
      Serial.print(" ");
      Serial.print(pulsetime[prevoneminitespos]);
      Serial.print("/");
      Serial.print(pc);

      interval = pulsetime[pulsetimepos] - prevpulsetime;
      pcd = (pcd_avg + (60000 / interval)) /2;

      prevpulsetime = pulsetime[pulsetimepos];
      pcd_avg = pcd;
      pulsetimepos++;
      if (ptsize <= pulsetimepos) {
        pulsetimepos = 0;
      }
      ucount = 0;
    }
    return p ;
  }
  int getInterval() {
    return interval;
  }
  int getPulseCount() {
    return pc;
  }
  int getEPulseCount() {
    return pcd;
  }
};

Pulse  pulse(analogpin0) ;
Pulse  breath(analogpin1) ;
FingerPulseMotor  m1(motor1p,motor1n);
FingerPulseMotor  m2(motor2p,motor2n);

void setup()
{
  Serial.begin(115200);
  Serial.print("\r\nStart");
  unsigned long now = millis();

  pulse.initpin();
  pulse.inittime(now);
  breath.initpin();
  breath.inittime(now);
  m1.initpin();
  m1.inittime(now);
  m2.initpin();
  m2.inittime(now);



  Serial.print("\r\nacc.powerOn()");
  acc.powerOn();
}

void loop()
{
  unsigned long now = millis();

  if (acc.isConnected()) {

    m1.update(now);
    m2.update(now);

    byte msg[7];
    int len = acc.read(msg, sizeof(msg), 1);
    while (0 < len) {
      switch (msg[0]) {
      case 0x51:
        Serial.print("\r\nreset time()");
        pulse.inittime(now);
        breath.inittime(now);
        msg[0] = 0x61;
        msg[1] = 0;
        msg[2] = 0;
        msg[3] = 0;
        msg[4] = 0;
        msg[5] = 0;
        msg[6] = 0;
        acc.write(msg, sizeof(msg));
        msg[0] = 0x67;
        acc.write(msg, sizeof(msg));
        break;
      case 0x61:
        {
          int retainlen = 7-1;
          int c = acc.read(&(msg[1]), retainlen, 1);
          m1.turnon(now);

        }
        break;
      case 0x67:
        {
          int retainlen = 7-1;
          int c = acc.read(&(msg[1]), retainlen, 1);
          m2.turnon(now);
        }
        break;
      }
      len = acc.read(msg, sizeof(msg), 1);
    }
    if (pulse.checkpulse(now)) {
      int  interval = pulse.getInterval();
      int  pc = pulse.getPulseCount();
      int  pcd = pulse.getEPulseCount();

      msg[0] = 0x61;
      msg[1] = (interval & 0xFFFF) >> 8;
      msg[2] = interval & 0xFF;
      msg[3] = (pcd & 0xFF00) >> 8;
      msg[4] = pcd & 0xFF;
      msg[5] = (pc & 0xFF00) >> 8;
      msg[6] = pc & 0xFF;
      acc.write(msg, sizeof(msg));
    }
    if (breath.checkpulse(now)) {
      int  interval = breath.getInterval();
      int  pc = breath.getPulseCount();
      int  pcd = breath.getEPulseCount();

      msg[0] = 0x67;
      msg[1] = (interval & 0xFFFF) >> 8;
      msg[2] = interval & 0xFF;
      msg[3] = (pcd & 0xFF00) >> 8;
      msg[4] = pcd & 0xFF;
      msg[5] = (pc & 0xFF00) >> 8;
      msg[6] = pc & 0xFF;
      acc.write(msg, sizeof(msg));
    }

  }
  else{
    pulse.inittime(now);
    breath.inittime(now);
    m1.turnoff();
    m2.turnoff();  
  }

  delay(30);
}










